// RIT Robotic Systems 2011-3 Project: Blue Steel
// Matt Hodge and Max Macri
//
#include <Ping.h> 
#include <Movement.h>

 // Constants
 const int AVG_CONST = 20;
 const int stop_inches = 9; // inches before an object to stop
 const int min_forwardTime = 1000; // (ms) Min Time to wander forward
 const int max_forwardTime = 5001; // (ms) (+mintime-1)
 const int min_turnTime = 100; // (ms) Min Time to turn while wandering
 const int max_turnTime = 2001; // (ms) (+mintime-1)
 const int max_irThreshold = 600; // (0-1024) Threashold to detect IR
 
 // Pins
 int leftIRPin = A3;
 int rightIRPin = A2;
 int RM_EN = 10;
 int LM_EN = 11;
 int LFWD = 12;
 int LREV = 13;
 int RFWD = 9;
 int RREV = 8;
 int leftPin = 5;
 int centerPin = 6;
 int rightPin = 7;
 long int leftNoiseValue[AVG_CONST];
 long int rightNoiseValue[AVG_CONST];
 long int leftSum = 0;
 long int rightSum = 0;
 float leftAvg = 0.0;
 float rightAvg = 0.0;
 
 // Primatives
 // Behavoir State Selection
 boolean pressed = false;
 boolean exit_loop = false;
 int state = 0;
 // Line
 int leftValue[2] = {0,0};
 int rightValue[2] = {0,0};
 int centerValue[2] = {0,0};
 // Wander
 int leftIRVal = 0;
 int rightIRVal = 0;
 long randMove = 0;
 long randTurn = 0;
 int hort_move = 0;
 
 // Objects
 Ping ping = Ping(4,0,29); //Pin,in_offset,cm_offset
 Movement move = Movement(LFWD,LREV,RFWD,RREV,LM_EN,RM_EN);

//
// Setup on Startup or Reset
//
void setup() {
  // Input
  pinMode(3,INPUT);
  pinMode(leftPin, INPUT);
  pinMode(centerPin, INPUT);
  pinMode(rightPin, INPUT);
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  
  attachInterrupt(1,changeState,RISING);

  state = 0;
  // Seed PRNG
  randomSeed(674);

}

//
// Main Loop - Check Button for Behavior State:
// Line Follower, IR Wanderer, Remote Driven
//
void loop() {
  exit_loop = false;
  // Run behavior state algorithm
  switch(state){
    case 0:
      line_follow();
      break;
    case 1:
      noise_drive();
      break;
    case 2:
      wander_ir();
      break;
  }
}

//
// Line Following Algorithm
//
void line_follow(){
  // set current LCR values to previous LCR values (only if line was previously spotted)
  if((leftValue[0]==0) || (centerValue[0]==0) || (rightValue[0] ==0)) {
    leftValue[1] = leftValue[0];
    centerValue[1] = centerValue[0];
    rightValue[1] = rightValue[0];
  }
  
  // read new LCR values
  leftValue[0] = digitalRead(leftPin);
  centerValue[0] = digitalRead(centerPin);
  rightValue[0] = digitalRead(rightPin);

  if((leftValue[0]==0) && (centerValue[0]==1) && (rightValue[0]==1)) {
    // Left
    move.setMove(85,25);
    move.setMove(85,25);
  }
  if((leftValue[0]==0) && (centerValue[0]==0) && (rightValue[0]==1)) {
    // Left/Center
    move.setMove(85,25);
    move.setMove(85,25);
  }
  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==1)) {
    // Center
    move.setMove(50,25);
    move.setMove(50,25);
  }  
  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==0)) {
    // Right/Center
    move.setMove(15,25);
    move.setMove(15,25);
  }
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==0)) {
    // Right
    move.setMove(15,25);
    move.setMove(15,25);    
  }
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==1)) {
    // No line found, check previous values
    if((leftValue[1]==1) && (centerValue[1]==1) && (rightValue[1]==0)) {
      // Previous value was Right
      move.setMove(25,50);
      move.setMove(25,50);
      while(digitalRead(centerPin)){
        delay(1);
      }
      move.setMove(50,25);
      move.setMove(50,25);
    }
    if((leftValue[1]==0) && (centerValue[1]==1) && (rightValue[1]==1)) {
      // Previous value was Left
      move.setMove(75,50);
      move.setMove(75,50);
      while(digitalRead(centerPin)) {
        delay(1);
      }
      move.setMove(50,25);
      move.setMove(50,25);
    }
  }
  delay(50);

  //delay(500);
}

//
// Wandering and IR Following Algorithm
//
void wander_ir() {
 int moveTime = 0;
  
  // move forward for a random time (mintime -to- (maxtime+mintime-1))
  randMove = random(max_forwardTime);
  randMove += min_forwardTime;
  
  // move forward for the generated time, avoiding objects, then turn
  while(moveTime < randMove && !exit_loop){
    // check for objects
    ping.fire();
    
    // check IR
    leftIRVal = analogRead(leftIRPin);
    rightIRVal = analogRead(rightIRPin);
    
    if(ping.inches() <= stop_inches) {
      // Object within range, turn to aviod
      move.setMove(90,50);
      moveTime = 0;
      
    }else if( leftIRVal <= max_irThreshold && rightIRVal <= max_irThreshold) { 
      // IR source has been spotted ahead, move towards it
      if(leftIRVal <= 350 && rightIRVal <=350) {
        // Move Straight Forward todo:mhodge mmacri
        move.setMove(50,35);
        
      }else{
        // Turn while moving forward
        hort_move = (rightIRVal-leftIRVal+max_irThreshold)/(max_irThreshold/50)+25;
        move.setMove(hort_move,35);
        delay(10);
        move.setMove(hort_move,35);
      }      
      moveTime = 0;
      
    }else if( leftIRVal <= max_irThreshold){ 
      // IR source has been spotted left, turn left towards it
      move.setMove(90,50);
      moveTime = 0;
      
    }else if( rightIRVal <= max_irThreshold){ 
      // IR source has been spotted right, turn right towards it
      move.setMove(10,50);
      moveTime = 0;
      
    } else {
      // Move forward for random time     
      move.setMove(50,35);
      moveTime += 50;
    }
    delay(50);
  }
  randTurn = random(max_turnTime);
  randTurn += min_turnTime;
    
  // (need to call setMove twice)
  move.setMove(90,50);
  delay(50);
  move.setMove(90,50);
  delay(randTurn); 
}

//
// Nosie Drive Algorithm
//
void noise_drive() {
  leftSum = 0;
  rightSum = 0;
  for(int i = 0; i<AVG_CONST; ++i)
  {
    leftNoiseValue[i] = analogRead(A1);
    rightNoiseValue[i] = analogRead(A0);
  }
  
  for(int i = 0; i<AVG_CONST; ++i)
  {
    leftSum += leftNoiseValue[i];
    rightSum += rightNoiseValue[i];
  }
  
  leftAvg = ((float)leftSum) / ((float)AVG_CONST);
  rightAvg = ((float)rightSum) / ((float)AVG_CONST);
  
  Serial.print(leftSum);
  Serial.print(" | ");
  Serial.print(rightSum);
  Serial.print(" | ");
  Serial.println(leftSum - rightSum);
  if((leftSum - rightSum) < -100.0)
  {
    move.setMove(20,50);
    move.setMove(20,50);
    delay(500);
  }
  if((leftSum - rightSum) > 100.0)
  {
    move.setMove(80,50);
    move.setMove(80,50);
    delay(500);
  }
  move.setMove(50,50);
  move.setMove(50,50);
}

//
// Change State
//
void changeState()
{
  state += 1;
  state = state%3;
  exit_loop = true;
}
